#include "mass_spring.h"

Mass_Spring::Mass_Spring(std::vector<Mass>& _m, std::vector<Spring>& _s) : System(_m.size()), masses(_m), springs(_s){
}

Mass_Spring::~Mass_Spring(){
}

Mass Mass_Spring::getMass(unsigned int _index) const{
	return masses.at(_index);
}

Spring Mass_Spring::getSpring(unsigned int _index) const{
	return springs.at(_index);
}

unsigned int Mass_Spring::sizeMasses() const {
	return masses.size();
}

unsigned int Mass_Spring::sizeSprings() const {
	return springs.size();
}

void Mass_Spring::draw() const {
	drawWalls();
	
	unsigned int sizeM = sizeMasses();
	for (unsigned int i = 0; i < sizeM; i++){
		drawMass(i);
	}
	unsigned int sizeS = sizeSprings();
	for (unsigned int i = 0; i < sizeS; i++){
		drawSpring(i);
	}
}

void Mass_Spring::drawWalls() const{
	//desenhando parede esquerda
	glColor3f(1.0, 1.0, 1.0);
	Vector3D wallBegin(getSpring(0).getPosBegin());
	
	glBegin(GL_LINES);
	glVertex3f(wallBegin.x, wallBegin.y + 1.0, wallBegin.z);
	glVertex3f(wallBegin.x, wallBegin.y - 1.0, wallBegin.z);
	
	for (double i = 1.0; i >= -1.0; i = i - 0.5){
		glVertex3f(wallBegin.x, wallBegin.y + i, wallBegin.z);
		glVertex3f(wallBegin.x - 0.2, wallBegin.y + i - 0.15, wallBegin.z);
	}
	glEnd();
	
	//desenhando parede direita
	Vector3D wallEnd(getSpring(sizeSprings()-1).getPosEnd());
	
	glBegin(GL_LINES);
	glVertex3f(wallEnd.x, wallEnd.y + 1.0, wallEnd.z);
	glVertex3f(wallEnd.x, wallEnd.y - 1.0, wallEnd.z);
	
	for (double i = 1.0; i >= -1.0; i = i - 0.5){
		glVertex3f(wallEnd.x, wallEnd.y + i, wallEnd.z);
		glVertex3f(wallEnd.x + 0.2, wallEnd.y + i - 0.15, wallEnd.z);
	}
	glEnd();
}

void Mass_Spring::drawMass(unsigned int _i) const{
	Mass m = getMass(_i);
	Vector3D position(m.getPosition() + m.getX());
	///////////////////////////////***///////////////////////////////
	double p = m.getWeight()/100.;
	///////////////////////////////***///////////////////////////////
 	glColor3f(m.getColor().r,m.getColor().g,m.getColor().b);

	glBegin(GL_QUADS);
	glVertex3f(position.x+p, position.y+p, position.z);
	glVertex3f(position.x-p, position.y+p, position.z);
	glVertex3f(position.x-p, position.y-p, position.z);
	glVertex3f(position.x+p, position.y-p, position.z);
	glEnd();
}

void Mass_Spring::drawSpring(unsigned int _i) const{
	Spring s = getSpring(_i);
	Vector3D posBegin(s.getPosBegin() + s.getXBegin());
	Vector3D posEnd(s.getPosEnd() + s.getXEnd());
	
	glColor3f(s.getColor().r,s.getColor().g,s.getColor().b);
	glBegin(GL_LINES);
	glVertex3f(posBegin.x, posBegin.y, posBegin.z);
	glVertex3f(posEnd.x, posEnd.y, posEnd.z);
	glEnd();
}

void Mass_Spring::calculate(unsigned int t){
	unsigned int sizeM = sizeMasses();
	for (unsigned int i = 0; i < sizeM; i++){
		double dx = 0.0;
		for (unsigned int j = 0; j < sizeM; j++){
			dx += alphas.at(j) * getU(i, j) * sin((getW(j) * t) + gammas.at(j));
		}
		Vector3D d(dx, 0.0, 0.0);
		masses.at(i).setX(d);
		springs.at(i).setXEnd(d);
		springs.at(i+1).setXBegin(d);
	}
}

void Mass_Spring::initialPosition(){
	unsigned int sizeM = sizeMasses();
	for (unsigned int i = 0; i < sizeM; i++){
		getMass(i).setX(Vector3D());
		getSpring(i).setXEnd(Vector3D());
		getSpring(i+1).setXBegin(Vector3D());
	}
}

void Mass_Spring::populateK(Matrix& k){
	unsigned int sizeM = sizeMasses();
	for (unsigned int i = 0; i < sizeM; i++){
		for (unsigned int j = 0; j < sizeM; j++){
			if (i == j)
				k[i][j] = getSpring(i).getK() + getSpring(i + 1).getK();
			else if (i - j == 1)
				k[i][j] = -getSpring(i).getK();
			else if (j - i == 1)
				k[i][j] = -getSpring(i + 1).getK();
			else
				k[i][j] = 0.0;
		}
	}
}

void Mass_Spring::populateM(Matrix& m){
	for(unsigned int i = 0; i < sizeMasses(); i++){
		m[i][i] = getMass(i).getWeight();
	}
}
